/*
 * sk_gui.c
 *
 *  Created on: Feb 28, 2011
 *      Author: asish
 */


#include <highgui.h>
#include <stdio.h>
#include <cv.h>
#include <pthread.h>
#include "sk_gui.h"
#include "sk_compute_trans.h"
#include "station_keeping.h"
#include "sk_polygon.h"
#include "sk_track_object.h"
#include "sk_util.h"


static char g_camInitialized = 0;
static CvPoint g_camA;
static CvPoint g_camB;
static IplImage *g_camFrame;
static CvFont g_font;
static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
static void sk_on_mouseCam( int event, int x, int y, int flags, void* param );
static void sk_displayText(IplImage *l_img,char *l_text, int l_line);
static void sk_displayError(IplImage *l_img,Deviation_t *l_error);
static IplImage * sk_GetCroppedImage(IplImage *in, CvPoint *pointA,CvPoint *pointB);

/*
 * Initialize the Graphical User Interface
 */
void
SK_InitGUI(char *argv[])
{
	char c;
	static CvCapture *capture;
	IplImage *polyImg = NULL;
	IplImage *polyImg1 = NULL;
	CvPoint2D32f initialPoints[4];
	CvPoint2D32f *ptsPtr;
	CvPoint2D32f *currentPoints;
	CvPoint2D32f lastBestPoints[4];
	char *status;
	int noOfPointsTracked;
	int noOfInitialPoints;
	Deviation_t *deviation;
	CvPoint *poly[1];
	CvPoint polyPts[4];
	int npts[1],i;
	CvMat *l_affineMat;
	CvVideoWriter *writer;
	CvSize l_size;
	char l_objectLost = 1;
	int l_lostObjCount = 0;
	CvScalar color;
	int iter = 0;

	if (strlen(argv[1]) == 1)
	{
		/*
		 * Create Camera Capture
		 */
		capture = cvCreateCameraCapture(atoi(argv[1]));
	}
	else
	{
		/*
		 * Create File Capture
		 */
		capture = cvCreateFileCapture(argv[1]);
	}

	if (!capture)
	{
		printf("Could not initialize capturing\n");
		return;
	}
	cvNamedWindow( "Input", CV_WINDOW_AUTOSIZE );
	cvSetMouseCallback( "Input", sk_on_mouseCam, 0 );
	cvInitFont(&g_font,CV_FONT_HERSHEY_SIMPLEX,1.0,1.0,0,3,CV_AA);

	g_camA = cvPoint(0,0);
	g_camB = cvPoint(0,0);
	/* Initialize writer */
	l_size = cvSize(
	  (int)cvGetCaptureProperty( capture, CV_CAP_PROP_FRAME_WIDTH),
	  (int)cvGetCaptureProperty( capture, CV_CAP_PROP_FRAME_HEIGHT)
	  );
	writer = cvCreateVideoWriter("output.avi",CV_FOURCC('P','I','M','1'),25,l_size,1);

	while (1)
	{
	    if( (char)c == 27 )
	    {
	    	cvReleaseCapture( &capture );
	    	return;
	    }

	    /*
	     * Wait until the user initializes by drawing a bounding box.
	     */
		while (!g_camInitialized)
		{
			g_camFrame = cvQueryFrame(capture);
			if (g_camFrame == NULL)
			{
				cvReleaseCapture( &capture );
				return;
			}
			else
			{
				pthread_mutex_lock(&mutex);
				cvRectangle(g_camFrame,g_camA,g_camB, CV_RGB(0,255,0),1,8,0);
				pthread_mutex_unlock(&mutex);
				sk_displayText(g_camFrame,"Not Initialized",1);
				cvShowImage("Input",g_camFrame);
				cvWriteFrame( writer, g_camFrame );
			}
		    c = cvWaitKey(20);
		    if( (char)c == 27 )
		    {
		    	cvReleaseCapture( &capture );
		    	return;
		    }

		}
		g_camFrame = cvQueryFrame(capture);
		if (g_camFrame == NULL)
		{
			break;
		}

		sk_displayText(g_camFrame,"Initializing...",1);
		cvShowImage("Input",g_camFrame);
		cvWriteFrame( writer, g_camFrame );
	    c = cvWaitKey(20);
	    if( (char)c == 27 )
	    {
	    	cvReleaseCapture( &capture );
	    	return;
	    }

		if (g_camFrame == NULL)
		{
			cvReleaseCapture( &capture );
			return;
		}
		if (polyImg == NULL)
		{
			polyImg = cvCreateImage( cvGetSize(g_camFrame), 8, 3 );
			polyImg1 = cvCreateImage( cvGetSize(g_camFrame), 8, 3 );
		}
		if ((noOfInitialPoints = SK_InitPolygon(g_camFrame,&g_camA,&g_camB,&ptsPtr)) == 4) // Detect contour and approximate polygon.
		{

			SK_InitObjectTracking(g_camFrame,ptsPtr,noOfInitialPoints); // Initialize Lucas Kanade
			for (i = 0; i< noOfInitialPoints; ++i)
			{
				memcpy(&(initialPoints[i]),&(ptsPtr[i]),sizeof(CvPoint2D32f));
				memcpy(&(lastBestPoints[i]),&(ptsPtr[i]),sizeof(CvPoint2D32f));
			}
			SK_InitAffineTrans(); // Initialize Affine Matrix Computation

			cvZero(polyImg);
			for (i = 0; i<4; ++i)
			{
				polyPts[i] = cvPointFrom32f(initialPoints[i]);

			}
			poly[0] = polyPts;
			npts[0] = noOfInitialPoints;
			cvFillPoly(polyImg,poly,npts,1,CV_RGB(255,0,255),CV_AA,0);
			sk_displayText(g_camFrame,"Initialized",1);
			l_objectLost = 0;
			while (1)
			{
				iter = (iter + 1)%RUNS_THRESH;
				if ((!l_objectLost) && ((noOfPointsTracked = SK_TrackObject(g_camFrame,&status,&currentPoints)) >= 3))
				{
					for (i = 0; i< noOfInitialPoints; ++i)
					{
						memcpy(&(lastBestPoints[i]),&(currentPoints[i]),sizeof(CvPoint2D32f));
					}
					l_lostObjCount = 0;
					color = CV_RGB(100,255,100);
				}
				else
				{
					l_objectLost = 1;
					if (l_lostObjCount > 0)
					{
						// try to find object back
						CvPoint pointA,pointB;
						pointA.x = (g_camA.x - l_lostObjCount*5)>0?(g_camA.x - l_lostObjCount*5):0;
						pointA.y = (g_camA.y - l_lostObjCount*5)>0?(g_camA.y - l_lostObjCount*5):0;
						pointB.x = (g_camB.x + l_lostObjCount*5)>0?(g_camB.x + l_lostObjCount*5):0;
						pointB.y = (g_camB.y + l_lostObjCount*5)>0?(g_camB.y + l_lostObjCount*5):0;
						if ((noOfInitialPoints = SK_InitPolygon(g_camFrame,&pointA,&pointB,&ptsPtr)) == 4) // Detect contour and approximate polygon.
						{

							SK_InitObjectTracking(g_camFrame,ptsPtr,noOfInitialPoints); // Initialize Lucas Kanade
							for (i = 0; i< noOfInitialPoints; ++i)
							{
								memcpy(&(lastBestPoints[i]),&(ptsPtr[i]),sizeof(CvPoint2D32f));
							}
							l_lostObjCount = 0;
							l_objectLost = 0;
						}
					}
					l_lostObjCount++;
					color = CV_RGB(255,100,100);
				}
				cvZero(polyImg1);
				cvZero(polyImg);
				cvFillPoly(polyImg,poly,npts,1,color,CV_AA,0);

				deviation = SK_ComputeError(initialPoints,status,lastBestPoints); // Get Affine Transformation matrix

				l_affineMat = SK_GetAffineMatrix();
				cvWarpAffine(polyImg,polyImg1,l_affineMat,CV_WARP_FILL_OUTLIERS,cvScalarAll(0));
				cvSetImageROI(polyImg1,cvRect(0,0,polyImg1->width,polyImg1->height));
				cvAddWeighted(g_camFrame,0.5,polyImg1,0.5,0.0,g_camFrame);
				sk_displayError(g_camFrame,deviation);
				cvShowImage("Input",g_camFrame);
				cvWriteFrame( writer, g_camFrame );
				if (iter)
					c = cvWaitKey(15);
			    if( (char)c == 27 )
			    {
			    	cvReleaseCapture( &capture );
			    	return;
			    }
			    if ((char)c == 'r')
			    	break;

				g_camFrame = cvQueryFrame(capture);
				if (g_camFrame == NULL)
				{
					cvReleaseCapture( &capture );
					return;
				}

			}
			g_camInitialized = 0;
			g_camA = cvPoint(0,0);
			g_camB = cvPoint(0,0);
		}
		else
		{
			/*
			 * Do Nothing
			 */
		}
	}
	cvReleaseCapture( &capture );
	cvReleaseVideoWriter(&writer);
}

/*
 * Mouse Listner for drawing the bounding box.
 */
void
sk_on_mouseCam( int event, int x, int y, int flags, void* param )
{
	static char l_isClicked = 0;

    if( g_camFrame->origin )
        y = g_camFrame->height - y;

    pthread_mutex_lock(&mutex);

    if( event == CV_EVENT_LBUTTONDOWN )
    {
    	if (l_isClicked == 0)
    	{
    		l_isClicked = 1;
    		g_camA = cvPoint(x,y);
    		g_camInitialized = 0;
    	}
    	if (l_isClicked == 1)
    	{
        	if (x > g_camA.x && y > g_camA.y)
        	{
				l_isClicked = 0;
				g_camB = cvPoint(x,y);
				g_camInitialized = 1;
        	}
    	}
    }
    else if (event == CV_EVENT_MOUSEMOVE)
    {
    	if (x > g_camA.x && y > g_camA.y)
			if (l_isClicked == 1)
			{
				g_camB = cvPoint(x,y);
			}
    }
    pthread_mutex_unlock(&mutex);
}


/*
 * Displays text on the image on the given line number from bottom.
 */
void
sk_displayText(IplImage *l_img,char *l_text, int l_line)
{
	cvPutText(l_img,l_text,cvPoint(0,l_img->height - 30*l_line),&g_font,CV_RGB(255,255,255));
}

/*
 * Displays error statistics
 */
void
sk_displayError(IplImage *l_img,Deviation_t *l_error)
{
	char l_buffer[255];
	sprintf(l_buffer,"Min Error: a = %6.2f, d = %6.2f",l_error->minAngle,l_error->minDisp);
	sk_displayText(l_img,l_buffer,3);
	sprintf(l_buffer,"Max Error: a = %6.2f, d = %6.2f",l_error->maxAngle,l_error->maxDisp);
	sk_displayText(l_img,l_buffer,2);
	sprintf(l_buffer,"Mean Error: a = %6.2f, d = %6.2f",l_error->rmsAngle,l_error->rmsDisp);
	sk_displayText(l_img,l_buffer,1);
}

/*
 * Crops the specified according to the rectangle specified by the two points.
 */
IplImage *
sk_GetCroppedImage(IplImage *in, CvPoint *pointA,CvPoint *pointB)
{
	IplImage *out;
	cvSetImageROI(in,cvRect(pointA->x,pointA->y,pointB->x - pointA->x,pointB->y - pointA->y));
	out = cvCreateImage(cvSize(pointB->x - pointA->x,pointB->y - pointA->y),8,3);
	cvCopy(in,out,0);
	cvResetImageROI(in);
	return out;
}
